/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SmartDashboard;
import edu.wpi.first.wpilibj.Solenoid;


/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Team997Protobot extends SimpleRobot {

    private static final int kDigitalSlot = 4;
    private static final int kSolenoidSlot = 7;
    private Joystick m_leftStick = new Joystick(1);
    private Joystick m_rightStick = new Joystick(2);
    // These are SpeedController objects, so the motors can use Jaguar or CANJaguar
    private SpeedController m_frontLeftMotor;
    private SpeedController m_frontRightMotor;
    private SpeedController m_rearLeftMotor;
    private SpeedController m_rearRightMotor;
    private Team997CubicDrive m_drivetrain;
    private DigitalInput m_autoSwitch[] = new DigitalInput[4];
    private Solenoid m_highGearSolenoid = new Solenoid(kSolenoidSlot, 1);
    private Solenoid m_lowGearSolenoid = new Solenoid(kSolenoidSlot, 2);
    private Solenoid m_deployOff = new Solenoid(kSolenoidSlot, 3);
    private Solenoid m_deployOn = new Solenoid(kSolenoidSlot, 4);
    private static final boolean kGearOff = false;
    private static final boolean kGearOn = !kGearOff;
    private static final boolean kLowGear = false;
    private static final boolean kHighGear = !kLowGear;
    //Enabling this will disable standard dashboard.
    private static final boolean kEnableSmartDashboard = true;
    private static final int kAutoSwitch1Channel = 1;
    private Compressor m_compressor = new Compressor(kDigitalSlot, 11, kDigitalSlot, 1);

    private void logSmartDashboard(String key, String value) {
        if (!kEnableSmartDashboard) {
            return;
        }
        int nRetCode = SmartDashboard.log(value, key);
        if (nRetCode != SmartDashboard.SUCCESS) {
            System.out.println("Error: " + SmartDashboard.diagnoseErrorCode(nRetCode));
        }
    }

    public void robotInit() {
        System.out.println("Build complete!");

        for (int i = 0; i < 4; i++) {
            m_autoSwitch[i] = new DigitalInput(kDigitalSlot, kAutoSwitch1Channel + i);
        }

        DriverStationPrinter.getInstance().println("Build complete!");
        if (kEnableSmartDashboard) {
            SmartDashboard.init();
        }
        logSmartDashboard("System State", "Init");
        m_frontLeftMotor = new Jaguar(kDigitalSlot, 6);
        m_frontRightMotor = new Jaguar(kDigitalSlot, 3);
        m_rearLeftMotor = new Jaguar(kDigitalSlot, 5);
        m_rearRightMotor = new Jaguar(kDigitalSlot, 4);

        m_drivetrain = new Team997CubicDrive(m_frontLeftMotor, m_rearLeftMotor, m_frontRightMotor, m_rearRightMotor);

        m_compressor.start();
    }

    public void disabled() {
        logSmartDashboard("System State", "Disabled");
    }

    public double round(double num, int digits) {
        for (int i = 0; i < digits; i++) {
            num *= 10;
        }
        num = (int) Math.floor(num + 0.5);

        for (int i = 0; i < digits; i++) {
            num /= 10;
        }
        return num;
    }

    public int getAutoState() {
        int nSwitchState = 0;
        for (int i = 0; i < 4; i++) {
            if (!m_autoSwitch[i].get()) {
                nSwitchState |= 1 << i;
            }
        }
        return nSwitchState;
    }

    /*
     * Autonomous Setting 1
     *
     * 1. Go forward on straight tape.
     * 2. Hang UBER on top, middle hook
     *
     * Autonomous Setting 2
     *
     * 1. Go forward on middle tape.
     * 2. Turn left to follow tape.
     * 3. Robot turns to face rack.
     * 4. Hang UBER on top hook.
     *
     * Autonomous Setting 3
     *
     * 1. Go forward on middle tape.
     * 2. Turn right to follow tape.
     * 3. Robot turns to face rack.
     * 4. Hang UBER on top hook.
     *
     * Autonomous Settings 4-6 inclusive
     *
     * Same thing as Settings 1-3 inclusive execpt for hang ring on middle rack.
     *
     * Autonomous Settings 7-9 inclusive
     *
     * Same thing as Settings 1-3 inclusive execpt for hang ring on lower rack.
     *
     * Autonomous Setting 0
     *
     * Do nothing.
     */
    public void autonomous() {
        logSmartDashboard("System State", "Auto");
        return;
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        shiftGear(kLowGear);
        getWatchdog().setEnabled(false);
        logSmartDashboard("System State", "TeleOp");


        while (isEnabled() && isOperatorControl()) {

            m_drivetrain.tankDrive(m_leftStick, m_rightStick);

            if (m_leftStick.getRawButton(2)) {
                shiftGear(kLowGear);
            } else if (m_leftStick.getRawButton(3)) {
                shiftGear(kHighGear);
            }

            if (m_rightStick.getRawButton(5))
                m_drivetrain.setReverse(false);
            else if (m_leftStick.getTrigger())
                m_drivetrain.setReverse(true);

            if (m_leftStick.getRawButton(11) || m_rightStick.getRawButton(11)) {
                deploy(true);
            } else if (m_leftStick.getRawButton(10) || m_rightStick.getRawButton(10)) {
                deploy(false);
            }

        }
    }

    private void updateSmartDashboard() {
      
    }

    /**
     * Shifts gears.
     * @param gear The gear (low vs. high)
     */
    private void shiftGear(boolean gear) {
        if (gear == kLowGear) {
            m_highGearSolenoid.set(kGearOff);
            m_lowGearSolenoid.set(kGearOn);
        } else {
            m_lowGearSolenoid.set(kGearOff);
            m_highGearSolenoid.set(kGearOn);
        }
    }

    private void deploy(boolean bDeploy) {
        if (bDeploy == false) {
            m_deployOff.set(true);
            m_deployOn.set(false);
        } else {
            m_deployOff.set(false);
            m_deployOn.set(true);
        }
    }
}
